/*
 * Created on 18 novembre 2005
 * bredeche(at)lri.fr
 *
 * This is demo for loading a map from an image file in Simbad . 
 * 
 * The important parts are:
 * - the main class, which describes the environment and setup the robot
 * - the robot class, which is embedded in the latter
 *  
 */

package contribs.maploader;

import java.io.FileNotFoundException;
import java.io.FileReader;
import java.util.ArrayList;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import simbad.gui.Simbad;
import simbad.sim.Agent;
import simbad.sim.BallAgent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;

/**
 * This class represents an Environment useable in Simbad but with the
 * specification of being defined by a PNG image. For the moment, the image is
 * defined as follows - 20x20 pixels (have to fit) - a red (rgb 255,0,0) pixel
 * means a box - a green (rgb 0,255,0) pixel represent a goal to reach (if any) -
 * a blue (rgb 0,0,255) pixel represents the starting position - dark gray (rgb
 * 60,60,60) pixel representing a simulated ball - the image have to be in a
 * PNG, JPG or GIF format.
 * 
 * TODO : - define more flexible constraints (20x20 -> ~ [10:50]x[10;50]) or
 * something like that (for now : ([0:20],[0:20]) values but it should be more
 * likely ([10:20],[10:20])) - add a niveler due to the rgb values for more
 * visual effects - add the specific (table) walls defined by some intermediary
 * color.
 * 
 * 
 * ADDED : - multi floor : example : if you load image.png, the app will search
 * for image-2.png for the second floor, image-3.png for the third, etc. -
 * Multi-robot instances
 * 
 * @author nicolas (+ cedric)
 * 
 */

public class MapLoaderDemo extends EnvironmentDescription {

	// static private Simbatch sim;
	// private Robot robot;

	// a goal to reach
	public Point3d _goal = new Point3d();
	// the initial starting points for robots (contains Point3d)
	public ArrayList _startingPoints = new ArrayList();

	public MapLoaderDemo() {
		// build the environment
		// here a simple square-closed environment.
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this);
		w1.rotate90(1);
		add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this);
		w2.rotate90(1);
		add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this);
		add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this);
		add(w4);

		// create the robot

		add(new Robot(new Vector3d(0, 0, 0), "MapRobot"));

	}

	/**
	 * initialize the environment due to the file __filename. TODO : - create
	 * multi-objectives points - create balls or non static items.
	 * 
	 * @param __filename
	 */
	public MapLoaderDemo(String __filename) {
		int[] values;

		// initialise the image
		SimpleImage simpleImage = new SimpleImage(__filename, false);
		simpleImage.displayInformation();

		// step 2 : for each data in the image, initialize the environment
		// + starting position (for one or more robots)
		// + goal position
		for (int y = 0; y != simpleImage.getHeight(); y++) {
			System.out.print(" ");
			for (int x = 0; x != simpleImage.getWidth(); x++) {
				values = simpleImage.getPixel(x, y);
				if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
					// red value, we will display a Wall in here :
					add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
					System.out.print("#");
				} else if (values[1] < 50 && values[2] > 200 && values[3] < 50) {
					// green value : define the goal point
					_goal.x = x;
					_goal.z = y;
					_goal.y = 0;
					System.out.print("X");
				} else if (values[1] < 50 && values[2] < 50 && values[3] > 200) {
					_startingPoints.add(new Point3d(x, 0, y));
					// starting position
					System.out.print("!");
				} else if (values[1] < 100 && values[1] == values[2] && values[2] == values[3]) {
					// add a ball
					// take care because the setUsePhysics remove the
					// agentInspector
					showAxis(false);
					setUsePhysics(true);
					add(new BallAgent(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), "ball", new Color3f(200, 0, 0), 0.25f, 0.25f));

				} else
					System.out.print(" ");
			}
			System.out.print("\n");
		}

		String secondFloor = __filename;
		boolean hasNextFloor = true;
		int cpt = 2;
		// add other floors to the environment
		// the other files should be called : for example if the initial file is
		// maze.png
		// - maze-2.png, maze-3.png, ....
		while (hasNextFloor) {
			try {
				// step 3 : define a second floor if exists
				if (__filename.endsWith(".png"))
					secondFloor = __filename.replaceAll(".png", "-" + cpt + ".png");
				if (__filename.endsWith(".gif"))
					secondFloor = __filename.replaceAll(".gif", "-" + cpt + ".gif");
				if (__filename.endsWith(".jpg"))
					secondFloor = __filename.replaceAll(".jpg", "-" + cpt + ".jpg");
				// only way found to check if the file exist
				// if it does not exists an exception is raised and we can
				// poursue without adding a second floor.
				new FileReader(secondFloor);

				// step 4 : initialise the image
				simpleImage = new SimpleImage(secondFloor, false);
				simpleImage.displayInformation();

				// step 5 : for each data in the image, update the environment
				for (int y = 0; y != simpleImage.getHeight(); y++) {
					System.out.print(" ");
					for (int x = 0; x != simpleImage.getWidth(); x++) {
						values = simpleImage.getPixel(x, y);
						if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
							// red value, we will display a Wall in here :
							add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), cpt - 1, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
							System.out.print("#");
						} else
							System.out.print(" ");
					}
					System.out.print("\n");
				}
				cpt++;
			} catch (FileNotFoundException fnfe) {
				// do nothing : do not add a second floor
				if (cpt == 2) {
					System.out.println("no second floor.");
					System.out.println(" - to define a second floor, create a file called : " + secondFloor);
					System.out.println("");
				}
				hasNextFloor = false;
			}
		}

		// add the robots of Robot instances
		for (int i = 0; i < _startingPoints.size(); i++) {
			add(new Robot(new Vector3d(((Point3d) _startingPoints.get(i)).x - (simpleImage.getWidth() / 2), 0f, ((Point3d) _startingPoints.get(i)).z - (simpleImage.getHeight() / 2)), "openDProbot"));
		}
	}

	// the robot is defined as an embedded class
	public class Robot extends Agent {

		RangeSensorBelt sonars, bumpers;

		public Robot(Vector3d position, String name) {
			super(position, name);
			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this, 8);
			bumpers = RobotFactory.addBumperBeltSensor(this, 12);
		}

		/** Initialize Agent's Behavior */
		@Override
		public void initBehavior() {
			// nothing particular in this case
		}

		/** Perform one step of Agent's Behavior */
		@Override
		public void performBehavior() {

			/*
			 * // *** clue-less robot *** if (collisionDetected()) { // stop the
			 * robot setTranslationalVelocity(0.0); setRotationalVelocity(0); }
			 * else { setTranslationalVelocity(1.5-2*Math.random());
			 * setRotationalVelocity(0.5-Math.random()); }
			 */

			// *** avoider robot ***
			if (bumpers.oneHasHit()) {
				setTranslationalVelocity(-0.1);
				setRotationalVelocity(0.5 - (0.1 * Math.random()));
			} else if (collisionDetected()) {
				// stop the robot
				setTranslationalVelocity(0.0);
				setRotationalVelocity(0);
			} else if (sonars.oneHasHit()) {
				// reads the three front quadrants
				double left = sonars.getFrontLeftQuadrantMeasurement();
				double right = sonars.getFrontRightQuadrantMeasurement();
				double front = sonars.getFrontQuadrantMeasurement();
				// if obstacle near
				if ((front < 0.7) || (left < 0.7) || (right < 0.7)) {
					if (left < right)
						setRotationalVelocity(-1);
					else
						setRotationalVelocity(1);
					setTranslationalVelocity(0);

				} else {
					setRotationalVelocity(0);
					setTranslationalVelocity(0.6);
				}
			} else {
				setTranslationalVelocity(0.8);
				;
				setRotationalVelocity(0);
			}

		}
	}

	/*
	 * for test purposes
	 */
	public static void main(String[] args) {
		MapLoaderDemo env = new MapLoaderDemo("resources/map1.png");
		new Simbad(env, false);
	}
}